#include "../include/SlamProcess.hpp"
#define _DEBUG
namespace ORBSLAM
{
    SlamProcess::SlamProcess(/* args */)
    {
        mm_velocity = Mat();

        mp_orbfeature = new ORBFeature(1.2, 8);
        mp_matcher = new Matcher();
        mp_matcher->Set_matcher_parameters(50,100, 0.9, true, 60);
        mp_tracker = new Track();
        mp_param = new Param();

        mp_preintegrater = make_unique<PreIntegrater>();

        Converter::Set_param(mp_param);
        mp_tracker->Set_param(mp_param);
        std::string filepath = "/home/heyrenqiang/vscode/test/data/ORBvoc.txt";
        mp_vocabulary = new DBoW3::Vocabulary();
        mp_vocabulary->load(filepath);
        mp_keyframedatabase = new KeyFrameDatabase(*mp_vocabulary);
        mp_mapatlas = new MapAtlas();
        mp_localmap = new LocalMap(mp_keyframedatabase);
        mp_localmap->Set_param(mp_param);
        mpt_localmapping = new thread(&ORBSLAM::LocalMap::Run, mp_localmap);
        mp_loopclosing = new LoopClosing(mp_keyframedatabase);
        mp_loopclosing->Set_param(mp_param);
        mpt_loopclosing = new thread(&ORBSLAM::LoopClosing::Run,mp_loopclosing);

        mp_localmap->Set_loopclosing(mp_loopclosing);
        mp_loopclosing->Set_localmap(mp_localmap);

        // mp_viewer = new Viewer();
        // mp_viewer->Set_mapatlas(mp_mapatlas);
        // mpt_viewer = new thread(&ORBSLAM::Viewer::Run,mp_viewer);

        mb_mono_initialized = false;
        mb_initial_frame_ready = false;
        me_state = NO_IMAGE;

        mn_para_maxkeyframe_th = 20;
        mn_para_minkeyframe_th = 0;
        mn_last_relocalization_frame_id = 0;
        mn_init_id = 0;
        mn_last_id = 0;
    }
    void SlamProcess::Process(const Mat &im,double time_stamp,vector<ImuDataPoint> v_imu_data)
    {
        if (!mb_mono_initialized)
        {
            if (!mb_initial_frame_ready)
            {
                mF_initial_frame = Frame(im, mp_orbfeature, 5000,time_stamp);
                mF_initial_frame.Set_preintegration(PreIntegrationData(),PreIntegrationData());
                if (mF_initial_frame.mn_feature_num > 100)
                {
                    mvp_prematched.resize(mF_initial_frame.mn_feature_num);
                    for (int i = 0; i < mF_initial_frame.mn_feature_num; i++)
                    {
                        mvp_prematched[i] = mF_initial_frame.mv_orb_unkeypoints[i].pt;
                    }
                    mb_initial_frame_ready = true;
                    mF_last_frame = mF_initial_frame;
                    return;
                }
                else{
                    return;
                }
            }
            else
            {
                mF_curframe = Frame(im, mp_orbfeature, 5000,time_stamp);
                m_preintegrationdata_lastframe = mp_preintegrater->Pre_integrate(PreIntegrationData(),mF_last_frame.m_time_stamp,mF_last_frame.m_bias,v_imu_data,time_stamp);
                m_preintegrationdata_lastkeyframe = mp_preintegrater->Pre_integrate(mF_last_frame.m_preintegrationdata_lastkeyframe,mF_last_frame.m_time_stamp,mF_last_frame.m_bias,v_imu_data,time_stamp);
                mF_curframe.Set_preintegration(m_preintegrationdata_lastframe,m_preintegrationdata_lastkeyframe);
                if (mF_curframe.mn_feature_num <= 100)
                {
                    mb_initial_frame_ready = false;
                    return;
                }
                int search_win_size = 100 + (mF_curframe.mn_frame_id - 1) * 10;
                mp_matcher->Set_ratio(0.5);

                vector<pair<int, int>> match_score;
                int nmateches = mp_matcher->Search_for_initialization(mF_initial_frame, mF_curframe, mvp_prematched, mvi_initial_matches, search_win_size, match_score);
                cout << "nmatches:" << nmateches << endl;

                if(nmateches<100){
                    mb_initial_frame_ready = false;
                    return;
                }
#ifdef _DEBUG
                Debuger::Show_two_image_match(mvi_initial_matches, mF_initial_frame, mF_curframe);
#endif
                Mat R21, t21;
                vector<bool> vb_triangulated;
                vector<int> v_contribution_pt1;
                vector<int> v_contribution_pt2;
                if (mp_tracker->Mono_initial_two_frame(mF_initial_frame.mv_orb_unkeypoints, mF_curframe.mv_orb_unkeypoints, mvi_initial_matches, R21, t21, mvp3f_initial3d, vb_triangulated, v_contribution_pt1, v_contribution_pt2, match_score))
                {

                    // Debuger::Show_two_image_match(mF_initial_frame,mF_curframe,v_contribution_pt1,v_contribution_pt2);

                    cout << "frame " << mF_initial_frame.mn_frame_id << " and frame " << mF_curframe.mn_frame_id << " initialized." << endl;
                    for (int i = 0; i < mvi_initial_matches.size(); i++)
                    {
                        if (mvi_initial_matches[i] >= 0 && !vb_triangulated[i])
                        {
                            mvi_initial_matches[i] = -1;
                            nmateches--;
                        }
                    }

                    vector<int> v_triangulated_p1;
                    vector<int> v_triangulated_p2;
                    v_triangulated_p1.reserve(mvi_initial_matches.size());
                    v_triangulated_p2.reserve(mvi_initial_matches.size());
                    for (int i = 0; i < mvi_initial_matches.size(); i++)
                    {
                        if (mvi_initial_matches[i] >= 0)
                        {
                            v_triangulated_p1.push_back(i);
                            v_triangulated_p2.push_back(mvi_initial_matches[i]);
                        }
                    }

                    Debuger::Show_two_image_match(mF_initial_frame, mF_curframe, v_triangulated_p1, v_triangulated_p2);

                    Mat pose_ini = Mat::eye(4, 4, CV_32F);
                    mF_initial_frame.Set_pose(pose_ini);
                    Mat Tcw = Mat::eye(4, 4, CV_32F);
                    R21.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));
                    t21.copyTo(Tcw.rowRange(0, 3).col(3));
                    mF_curframe.Set_pose(Tcw);
                    mF_last_frame = mF_curframe;
                    Create_momo_initial_map();
                    mb_mono_initialized = true;
                    me_state = OK;
                }
                else{
                    mF_last_frame = mF_curframe;
                    return;
                }
            }
        }
        else
        {
            clock_t start_time, end_time;
            // start_time = clock();
            if (mn_last_id - mn_init_id < mn_para_maxkeyframe_th)
            {
                mF_curframe = Frame(im, mp_orbfeature, 5000,time_stamp);
            }
            else
            {
                mF_curframe = Frame(im, mp_orbfeature, 1000,time_stamp);
            }
            // end_time = clock();
            // cout << "run Frame time: " << (double)(end_time - start_time) / CLOCKS_PER_SEC << "s" << endl;
            cout << "frame id.............................................................." << mF_curframe.mn_frame_id << endl;
            // sleep(10);
            Compute_bow(mF_curframe);
            mn_last_id = mF_curframe.mn_frame_id;
            if (me_state == OK)
            {
                bool b_ok;
                if (mm_velocity.empty())
                {

                    cout << "track reference keyframe..." << endl;
                    mF_curframe.Set_pose(mF_last_frame.Get_Tcw());
                    // TODO set matcher
                    mp_matcher->Set_ratio(0.7);
                    mp_matcher->Set_if_check_orientation(true);
                    b_ok = mp_tracker->Track_reference_keyframe(mp_ref_KF, mF_curframe, mp_matcher);
                    // Debuger::Show_two_image_match(mp_ref_KF, mF_curframe);
                }
                else
                {
                    cout << "track by projection..." << endl;
                    // TODO set matcher
                    mp_matcher->Set_ratio(0.9);
                    mp_matcher->Set_if_check_orientation(true);
                    Update_last_frame();
                    b_ok = mp_tracker->Track_by_motion_predict(mF_last_frame, mF_curframe, mp_matcher, mm_velocity);
                    if (!b_ok)
                    {
                        cout << "track by projection fail, track by keyframe..." << endl;
                        // Compute_bow(mF_curframe);
                        mF_curframe.Set_pose(mF_last_frame.Get_Tcw());
                        // TODO set matcher
                        mp_matcher->Set_ratio(0.7);
                        mp_matcher->Set_if_check_orientation(true);
                        b_ok = mp_tracker->Track_reference_keyframe(mp_ref_KF, mF_curframe, mp_matcher);
                    }
                }

                if (!b_ok)
                {
                    // TODO  relocalization
                    b_ok = mp_tracker->Relocalization(mF_curframe,mp_keyframedatabase,mp_matcher,mp_cur_map);
                    if(b_ok){
                        mn_last_relocalization_frame_id = mF_curframe.mn_frame_id;
                    }else{
                        me_state = LOST;
                    }
                }
                mp_ref_KF->Set_as_ref_keyframe(false);
                if (!mF_curframe.mp_ref_keyframe)
                {
                    mF_curframe.mp_ref_keyframe = mp_ref_KF;
                }
                if (b_ok)
                {
                    // TODO track local map
                    mp_localmap->Update_local_keyframe(mF_curframe);
                    mp_ref_KF = mF_curframe.mp_ref_keyframe;
                    mp_ref_KF->Set_as_ref_keyframe(true);
                    mp_localmap->Update_local_mappoint(mF_curframe);
                    // TODO set matcher
                    mp_matcher->Set_ratio(0.8);
                    b_ok = mp_tracker->Track_local_map(mp_localmap, mF_curframe, mp_matcher, mn_match_inline);
                    cout << "endl track loaclmap" << endl;
                    // Debuger::Show_two_image_match(mp_ref_KF, mF_curframe);
                }
                else
                {
                    me_state = LOST;
                }
                if (b_ok)
                {
                    // TODO velocity.
                    if (!mF_last_frame.mm_Tcw.empty() && !mF_curframe.mm_Tcw.empty())
                    {
                        mm_velocity = mF_curframe.Get_Tcw() * mF_last_frame.Get_Twc();
                        // mm_velocity = mF_curframe.mm_Tcw*mF_last_frame.mm_Twc;
                    }
                    else
                    {
                        mm_velocity = Mat();
                    }

                    bool b_need_new_keyframe = Need_new_keyframe();

                    if (b_need_new_keyframe)
                    {
                        // Debuger::Show_two_image_match(mp_ref_KF, mF_curframe);
                        Create_new_keyframe();
                        // mp_keyframedatabase->Add(mp_last_keyframe);
                    }

                    me_state = OK;
                }
                else
                {
                    me_state = LOST;
                }
            }
            if (me_state == OK)
            {
            }
            // if (!mF_curframe.mp_ref_keyframe)
            // {
            //     mF_curframe.mp_ref_keyframe = mp_ref_KF;
            // }
            mF_last_frame = mF_curframe;
            // cout << "ref keyframe id:" << mp_ref_KF->mn_KF_id << endl;
        }
        if (me_state == OK)
        {
            if (!mF_curframe.mm_Tcw.empty())
            {
                Mat Tcr = mF_curframe.Get_Tcw() * mF_curframe.mp_ref_keyframe->Get_Twc();
                mlm_relative_frame_pose.push_back(Tcr);
                mlp_reference_keyframe.push_back(mF_curframe.mp_ref_keyframe);
            }
        }
    }

    void SlamProcess::Create_momo_initial_map()
    {
        mp_cur_map = mp_mapatlas->Get_current_map();
        mp_localmap->Set_map(mp_cur_map);

        Compute_bow(mF_initial_frame);
        Compute_bow(mF_curframe);

        KeyFrame *pkF_ini = new KeyFrame(mF_initial_frame);
        KeyFrame *pkF_cur = new KeyFrame(mF_curframe);

        // swap(*((Frame *)pkF_ini), mF_initial_frame);
        // swap(*((Frame *)pkF_cur), mF_curframe);

        pkF_ini->Init_keyframe();
        pkF_cur->Init_keyframe();
        pkF_ini->Set_map(mp_cur_map);
        pkF_cur->Set_map(mp_cur_map);
        mp_cur_map->Add_key_frame(pkF_ini);
        mp_cur_map->Add_key_frame(pkF_cur);
        for (int i = 0; i < mvi_initial_matches.size(); i++)
        {
            if (mvi_initial_matches[i] < 0)
            {
                continue;
            }
            Mat world_pose(mvp3f_initial3d[i]);
            MapPoint *p_mappoint = new MapPoint(world_pose);
            p_mappoint->Set_ref_keframe(pkF_ini, pkF_cur);
            p_mappoint->Set_map(mp_cur_map);
            pkF_ini->Add_mappoint(p_mappoint, i);
            pkF_cur->Add_mappoint(p_mappoint, mvi_initial_matches[i]);
            p_mappoint->Add_observation(pkF_ini, i);
            p_mappoint->Add_observation(pkF_cur, mvi_initial_matches[i]);
            p_mappoint->Compute_distinctive_descriptors();
            p_mappoint->Update_normal_and_depth();
            mp_cur_map->Add_mappoint(p_mappoint);
            mp_localmap->mlp_recently_added_mappoints.push_back(p_mappoint);
        }

        pkF_ini->Update_connections();
        pkF_cur->Update_connections();

        cout << "before ba:" << endl;
        cout << "ini pose:" << endl
             << pkF_ini->mm_Tcw << endl;
        cout << "cur pose:" << endl
             << pkF_cur->mm_Tcw << endl;
        Optimizer::Global_bundle_adjustment(mp_cur_map, 20);
        cout << "after ba:" << endl;
        cout << "ini pose:" << endl
             << pkF_ini->mm_Tcw << endl;
        cout << "cur pose:" << endl
             << pkF_cur->mm_Tcw << endl;
        float median_depth = pkF_ini->Compute_scene_median_depth(2);
        float inv_median_depth;
        inv_median_depth = 4.0f / median_depth;
        if (median_depth < 0 || pkF_cur->Tracked_mappoints(1) < 50)
        {
            Reset_active_map();
            return;
        }
        Mat Tcw = pkF_cur->Get_Tcw();
        Tcw.rowRange(0, 3).col(3) = Tcw.col(3).rowRange(0, 3) * inv_median_depth;
        pkF_cur->Set_pose(Tcw);

        vector<MapPoint *> vp_all_mappoints = pkF_ini->Get_vect_mappoints();
        for (int i = 0; i < vp_all_mappoints.size(); i++)
        {
            if (vp_all_mappoints[i])
            {
                MapPoint *p_mp = vp_all_mappoints[i];
                p_mp->Set_world_pose(p_mp->Get_world_pose() * inv_median_depth);
                p_mp->Update_normal_and_depth();
            }
        }

        // Viewer::showTrajectoryandpoints(mp_cur_map);

        // cout << "after pangolin show.." << endl;

        pkF_cur->mp_keyframe_pre = pkF_ini;
        pkF_ini->mp_keyframe_next = pkF_cur;

        mp_localmap->Insert_keyframe(pkF_ini);
        mp_localmap->Insert_keyframe(pkF_cur);
        mvp_keyframe.push_back(pkF_ini);
        mvp_keyframe.push_back(pkF_cur);

        mp_last_keyframe = pkF_cur;
        mn_last_keyframe_frame_id = mF_curframe.mn_frame_id;
        mn_last_relocalization_frame_id = mF_initial_frame.mn_frame_id;

        mp_ref_KF = pkF_cur;
        mp_ref_KF->Set_as_ref_keyframe(true);
        mF_curframe.mp_ref_keyframe = mp_ref_KF;
        // mp_tracker->Set_ref_KF(pkF_cur);
        mn_init_id = mF_curframe.mn_frame_id;
    }

    void SlamProcess::Reset_active_map()
    {
    }

    void SlamProcess::Compute_bow(KeyFrame *pkf)
    {
        vector<Mat> v_cur_desc = Converter::toDescriptorVector(pkf->mm_descriptors);
        mp_vocabulary->transform(v_cur_desc, pkf->mv_bowvector, pkf->mv_featurevector, 4);
        // cout << "bow vector size:" << pkf->mv_bowvector.size() << endl;
        // cout << "feature vector size:" << pkf->mv_featurevector.size() << endl;
    }

    void SlamProcess::Compute_bow(Frame &frame)
    {
        vector<Mat> v_cur_desc = Converter::toDescriptorVector(frame.mm_descriptors);
        mp_vocabulary->transform(v_cur_desc, frame.mv_bowvector, frame.mv_featurevector, 4);
        // cout << "bow vector size:" << frame.mv_bowvector.size() << endl;
        // cout << "feature vector size:" << frame.mv_featurevector.size() << endl;
    }

    bool SlamProcess::Need_new_keyframe()
    {
        const int nkeyframes = mp_cur_map->Get_all_keyframe().size();
        if (mF_curframe.mn_frame_id < mn_last_relocalization_frame_id + mn_para_maxkeyframe_th && nkeyframes > mn_para_maxkeyframe_th)
        {
            return false;
        }
        int nminobs = 3;
        if (nkeyframes <= 2)
        {
            nminobs = 2;
        }
        int ntmp = mp_ref_KF->Sum_mappoints_by_obsnum(nminobs);

        float ratio_th = 0.8;

        bool b_localmap_idle = mp_localmap->Accept_keyframes();

        bool c1a = mF_curframe.mn_frame_id >= mn_last_keyframe_frame_id + mn_para_maxkeyframe_th;
        cout << "max keyframe condition:" << c1a << endl;

        bool c1b = (mF_curframe.mn_frame_id >= mn_last_keyframe_frame_id + mn_para_minkeyframe_th) && b_localmap_idle;
        cout << "idle condition:" << c1b << endl;

        bool c2 = mn_match_inline < ratio_th * ntmp && mn_match_inline > 15;
        cout << "match condition:" << c2 << endl;

        if ((c1a || c1b) && c2)
        {
            if (b_localmap_idle)
            {
                cout << "need create new keyframe..." << endl;
                return true;
            }
            else
            {
                mp_localmap->Interrupt_ba();
                if (mp_localmap->Keyframe_in_queue() < 3)
                {
                    cout << "need create new keyframe..." << endl;
                    return true;
                }
                else
                {
                    cout << "dont create, keyframe in queue size:" << mp_localmap->Keyframe_in_queue() << endl;
                    return false;
                }
            }
        }
        cout << "dont create keyframe, ref mpt num(obs>3):" << ntmp << endl;
        return false;
    }

    void SlamProcess::Create_new_keyframe()
    {
        KeyFrame *p_keyframe = new KeyFrame(mF_curframe);
        // swap(*((Frame *)p_keyframe), mF_curframe);
        p_keyframe->Init_keyframe();
        p_keyframe->Set_map(mp_cur_map);
        mp_cur_map->Add_key_frame(p_keyframe);

        mp_ref_KF->Set_as_ref_keyframe(false);

        mp_ref_KF = p_keyframe;

        mp_ref_KF->Set_as_ref_keyframe(true);

        mF_curframe.mp_ref_keyframe = mp_ref_KF;

        if (mp_last_keyframe)
        {
            p_keyframe->mp_keyframe_pre = mp_last_keyframe;
            mp_last_keyframe->mp_keyframe_next = p_keyframe;
        }
        else
        {
            cout << "no last key frame" << endl;
        }

        mp_localmap->Insert_keyframe(p_keyframe);
        mp_localmap->Set_not_stop(false);

        mn_last_keyframe_frame_id = mF_curframe.mn_frame_id;
        mp_last_keyframe = p_keyframe;
    }

    void SlamProcess::Update_last_frame()
    {
        KeyFrame *p_ref_keyframe = mF_last_frame.mp_ref_keyframe;
        Mat Tlr = mlm_relative_frame_pose.back();
        mF_last_frame.Set_pose(Tlr * p_ref_keyframe->Get_Tcw());
    }

}